#include <cv.h>
#include <highgui.h>

#include "HandInterface.hpp"
#include "TrackDataServer.hpp"

#include <boost/thread.hpp>

using namespace cv;
using namespace hi;

SingleTrackerRunner::SingleTrackerRunner( Ptr< AbstractTrackDataServer > trackDataServerPtr ) : trackDataServerPtr( trackDataServerPtr ),
		hl(0), ht(0), nextFrameReadyBar(2), computationCompleteBar(2) {
};

SingleTrackerRunner::~SingleTrackerRunner() {
	delete hl;
	delete ht;
}

int SingleTrackerRunner::init() {
	HiVisualiser::addWindow("handtracker");
	HiVisualiser::moveWindow("handtracker",500,200);
	
	capPtr = Ptr< VideoCapture >(new VideoCapture(0)); // open the default camera
	bool captureSizeSet = capPtr->set( CV_CAP_PROP_FRAME_WIDTH , 640 );
	captureSizeSet &= capPtr->set( CV_CAP_PROP_FRAME_HEIGHT , 480 );
	if(!capPtr->isOpened() || !captureSizeSet) { // check if we succeeded
		return -1;
	} else {
		return 0;
	}
}

void SingleTrackerRunner::run() {
	running = true;
	
	while (running && localise() == -1 ) {}
	if (!running) return;
	
	Ptr< GeometricGestureClassifier > internalClassifierPtr = Ptr< GeometricGestureClassifier >( new BoostGestureClassifier( "C0", "C0", hl->getProbabilityTransformer() ) );
	
	//paramset3
	ht = HandTracker::init( false, 10.0, 0.1, 100.0, 10.0,
						   trackingRect, frame, internalClassifierPtr );

	// -- run tracker -- //
	boost::thread handTrackerThread( boost::bind( &SingleTrackerRunner::runTracker, this ) );
	
	Rect trackingRectBuffer;
	bool isHandActiveBuffer;
	
	while (running) {
		trackingRectBuffer = trackingRect;
		isHandActiveBuffer = ht->isHandActive();
		setNextFrame();
		nextFrameReadyBar.wait(); //allow tracking of next rect to start while processing data from previous rect
		
		//tracking post-processing
		trackDataServerPtr->putTrackData( isHandActiveBuffer, trackingRectBuffer.x+trackingRectBuffer.width/2, trackingRectBuffer.y+trackingRectBuffer.height/2 );
		
		computationCompleteBar.wait(); //wait for tracking to complete
		waitKey(1);//helps visualiser run more smoothly
	}
}

void SingleTrackerRunner::runTracker() {
	// -- tracking hand --//
	while (running) {
		nextFrameReadyBar.wait();
		
		trackingRect = ht->track( frame );

		computationCompleteBar.wait();
	}
}

void SingleTrackerRunner::stop() { running = false; }

// -- protected methods -- //
void SingleTrackerRunner::setNextFrame() {
	(*capPtr) >> frame;
	flip(frame, frame, 1);
}

int SingleTrackerRunner::localise() {
	hl = new HandLocaliser();
	try {
		setNextFrame();
		//trackingRect = hl->localiseFace( frame );
		trackingRect = hl->localiseHand( frame );
		cout << "Face found" << endl;
	}
	catch (cv::Exception e) {
		if ( e.code == HI_ERR_NOFACEFOUND ) {
			delete hl;
			return -1;
		} else {
			throw;
		}
	}
	
	//forcing rectangle to stay inside frame
	forceInside( frame.size(), trackingRect );
	
	return 0;
}


